#include "FrcShooterSubsystem.h"
#include "FrcConstants.h"

FrcShooterSubsystem::FrcShooterSubsystem()
{
	m_shooterMotor = new Talon(c_shooterMotorPort);
	m_rightJoystick = Joystick::GetStickForPort(c_joystickRightPort);
	m_shooterTimer = new Timer();
	m_fireButton = new OperatorButton(2, c_fireButtonPort);
	m_resetButton = new OperatorButton(1, c_fireButtonPort);
	m_overrideButton = new OperatorButton (c_overrideButtonPort);
	m_shooterEncoder = new Encoder(13,14);
	m_manualBackwardShooterButton = new OperatorButton (c_manualBackwardShooterButtonPort);
	m_manualForwardShooterButton = new OperatorButton (c_manualForwardShooterButtonPort);
	m_shooterEncoder->SetDistancePerPulse(1); // one pulse every 1.5 degrees

	
}

FrcShooterSubsystem::~FrcShooterSubsystem()
{
	
}

void FrcShooterSubsystem::DoRobotInit(void)
{
	m_is15Reached = true;
	m_is30Reached = true;
	m_is45Reached = true;
}

void FrcShooterSubsystem::DoTeleopInit(void)
{
	m_shooterEncoder->Reset();
	m_shooterEncoder->Start();
	m_shooterState = kHolding;
}

void FrcShooterSubsystem::DoDisabledInit(void)
{
	m_shooterMotor->Set(0);
}

void FrcShooterSubsystem::DoTeleopPeriodic(void)
{
	 if (m_overrideButton->Get() == OperatorButton::kPressed)
	{
		ManualControl();
		m_shooterState = kHolding;
	} 
	else if (m_overrideButton->Get() == OperatorButton::kReleased)
	{
		ShooterControl();
	}   
	
	SmartDashboard::PutNumber("Shooter Encoder Distance: ", m_shooterEncoder->GetDistance());
	SmartDashboard::PutNumber("Shooter Encoder 2: ", m_shooterEncoder->GetRaw());
	SmartDashboard::PutNumber("Shooter Encoder 3: ", m_shooterEncoder->Get());
	SmartDashboard::PutBoolean("15 Seconds Passed", m_is15Reached);
	SmartDashboard::PutBoolean("30 Seconds Passed", m_is30Reached);
	SmartDashboard::PutBoolean("45 Seconds Passed", m_is45Reached);
	SmartDashboard::PutNumber("Time Since Loaded", m_shooterTimer->Get());
}

void FrcShooterSubsystem::DoDisabledPeriodic(void)
{
	
}

void FrcShooterSubsystem::ShooterControl()
{	

	
	switch (m_shooterState)
	{
	case kLoad:
		printf("Enter Load \n");
		m_shooterMotor->Set(-1);
		if(m_shooterEncoder->GetDistance() <= c_loadToLoadedDisance)
		{
			m_shooterMotor->Set(0);
			printf("Exit Load \n");
			m_shooterTimer->Stop();
			m_shooterTimer->Reset();
			m_shooterTimer->Start();
			m_is15Reached = true;
			m_is30Reached = true;
			m_is45Reached = true;
			m_shooterState = kLoaded; 
		}
		
	break;
	case kLoaded:
		printf("Enter Loaded \n");
		// safety check code (AKA: dont smoke the motor code)
		if(m_shooterTimer->Get() >= 15)
		{
			 m_is15Reached = false;
		}
		
		if(m_shooterTimer->Get() >= 30)
		{
			m_is30Reached = false;
		}
		
		if(m_shooterTimer->Get() >= 45)
		{
			m_is45Reached = false;
		}
		// Balancing code
		
		if(m_shooterEncoder->GetDistance() >= c_shooterUpperHoldingBound) 
		{
			m_shooterMotor->Set(-.35);
		}
		if(m_shooterEncoder->GetDistance() <= c_shooterLowerHoldingBound)
		{
			m_shooterMotor->Set(.35);
		}
		if (m_shooterEncoder->GetDistance() <= c_shooterUpperHoldingBound && m_shooterEncoder->GetDistance() >= c_shooterLowerHoldingBound)
		{
			m_shooterMotor->Set(0);
		}
		if(m_fireButton->Get() == OperatorButton::kJustPressed)
		{
			m_shooterMotor->Set(-1);
			printf("Exit Loaded \n");

			m_shooterState = kFire;
		}
	break;
	case kFire:
		printf("Enter Fire \n");

			if(m_shooterEncoder->GetDistance() <= c_loadedToFireDistance)
			{
				m_shooterMotor->Set(.1);
				m_shooterState = kReset;
			}
	break;
	case kReset:
		printf("Enter Reset \n");
		m_shooterMotor->Set(.35);
		if(m_shooterEncoder->GetDistance() >= c_armResetDistance)
		{
			printf("Exit Reset \n");
			m_shooterMotor->Set(0);
			m_shooterState = kHolding;
		}
	break; 
	
	case kHolding:
		if (m_resetButton->Get() == OperatorButton::kJustPressed)
		{
			m_shooterState = kLoad;
		}
		
		break;
	
	} 
				
}

void FrcShooterSubsystem::ManualControl()
{
	if (m_manualBackwardShooterButton->Get() == OperatorButton::kPressed && !(m_manualForwardShooterButton->Get() == OperatorButton::kPressed))
	{
		m_shooterMotor->Set(.35);
	} else if (m_manualForwardShooterButton->Get() == OperatorButton::kPressed && !(m_manualBackwardShooterButton->Get() == OperatorButton::kPressed))
	{
		m_shooterMotor->Set(-1 );
	}
	else
	{
		m_shooterMotor->Set(0);
	}
	
}
